#include "motor.h"
#include "appdata/app_data.h"
#include "keyboard/keyboard.h"
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(MC3, MC2, MC1, MC_EN);
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);

static XKnobConfig x_knob_configs[] = {
    // 任意运动的控制  有阻尼 类似于机械旋钮
    [MOTOR_NAVIGATE_MODE] = {0, 0, 30 * PI / 180, 0.7, 2, 0.8},
    // 240度圆弧用
    [MOTOR_USE_ARC] = {240, 0, 2 * PI / 180, 0.4, 2, 2},
    // 无限制  不制动
    [MOTOR_SUPER_DIAL] = {0, 0, 7 * PI / 180, 1.8, 2.6, 1.1},
    [MOTOR_UNBOUND_COARSE_DETENTS] = {0, 0, 8.225806452 * _PI / 180, 2.3, 1, 1.1},
    [MOTOR_BOUND_0_12_NO_DETENTS] = {13, 0, 10 * PI / 180, 0, 1, 1.1},
    [MOTOR_BOUND_LCD_BK_BRIGHTNESS] = {101, 10, 2 * PI / 180, 2, 1, 1.1},
    [MOTOR_BOUND_LCD_BK_TIMEOUT] = {31, 0, 5 * PI / 180, 2, 1, 1.1},
    // 粗糙的棘轮 强阻尼
    [MOTOR_COARSE_DETENTS] = {32, 0, 8.225806452 * PI / 180, 2, 1, 1.1},
    // 任意运动的控制  无阻尼
    [MOTOR_FINE_NO_DETENTS] = {256, 127, 1 * PI / 180, 0, 1, 1.1},
    // 模拟开关  强制动
    [MOTOR_ON_OFF_STRONG_DETENTS] = {2, 0, 60 * PI / 180, 1, 1, 0.55},
    // Note the snap point is slightly past the midpoint (0.5); compare to normal detents which use a snap point *past* the next value (i.e. > 1)
};

XKnobConfig motor_config = {
    .num_positions = 0,
    .position = 0,
    .position_width_radians = 30 * _PI / 180,
    .detent_strength_unit = 0.06,
    .endstop_strength_unit = 0.7,
    .snap_point = 1,
};

// 死区制动百分率
static const float DEAD_ZONE_DETENT_PERCENT = 0.008;
// 死区RAD?
static const float DEAD_ZONE_RAD = 4 * _PI / 180;
// 怠速速度ewma alpha
static const float IDLE_VELOCITY_EWMA_ALPHA = 0.001;
// 怠速速度每秒钟弧度
static const float IDLE_VELOCITY_RAD_PER_SEC = 0.008;
// 怠速修正延迟millis
static const uint32_t IDLE_CORRECTION_DELAY_MILLIS = 500;
// 怠速校正最大角度rad
static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
// 怠速修正率
static const float IDLE_CORRECTION_RATE_ALPHA = 0.001;

// 当前相对位置
float current_detent_center = 0;
// 上次空闲开始状态
uint32_t last_idle_start = 0;
// 怠速检查速度
float idle_check_velocity_ewma = 0;

// -------------monitor--------------------
// 目标变量
static float readMySensorCallback(void)
{
      float rad = sensor.getSensorAngle();
      return rad; // 角的单位是弧度[rad]
}
/*
 * 将随机变化的值限制在一个给定的区间[min,max]内
 */
static float CLAMP(const float value, const float low, const float high)
{
      return value < low ? low : (value > high ? high : value);
}

void motor_init(void *pvParameters)
{
      Wire.setPins(SDA, SCL);
      sensor.init();
      motor.linkSensor(&sensor);
      driver.pwm_frequency = 800000;
      driver.voltage_power_supply = 12;
      driver.voltage_limit = 12;
      driver.init();
      // link the motor and the driver
      motor.linkDriver(&driver);
      // choose FOC modulation (optional)
      motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
      // set motion control loop to be used

      motor.controller = MotionControlType::torque;
      // 速度PID控制器参数
      // 默认P=0.5 I=10 D=0
      motor.PID_velocity.P = 0.3;  // 比例
      motor.PID_velocity.I = 0.0;  // 积分  误差累积
      motor.PID_velocity.D = 0.03; // 微分  变化率
      // 速度低通滤波器
      // 默认为5ms - 可以不断尝试，获取最佳数值

      // 数值越低，滤波频率越小
      motor.LPF_velocity.Tf = 0.07;

      // 设置限制
      // 电压限制
      motor.voltage_limit = 12; // Volts - 默认为驱动器电压限制

      motor.init();
      // align sensor and start FOC
      motor.initFOC();
#if XK_INVERT_ROTATION
      current_detent_center = -motor.shaft_angle;
#else
      current_detent_center = motor.shaft_angle;
#endif
      update_motor_mode(MOTOR_NAVIGATE_MODE, 0);
      while (1)
      {
            sensor.update();
            motor.loopFOC();

            idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);

            if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC)
            {
                  last_idle_start = 0;
            }
            else
            {
                  if (last_idle_start == 0)
                  {
                        last_idle_start = millis();
                  }
            }

            // 如果我们没有移动，并且我们接近中心(但不是完全在那里)，慢慢调整中心点以匹配当前位置
            // If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
            if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD)
            {
                  // Serial.println("slowly adjust the centerpoint to match the current position......");
                  current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
            }

            // 到控制中心的角度 差值
#if XK_INVERT_ROTATION
            float angle_to_detent_center = -motor.shaft_angle - current_detent_center;
#else
            float angle_to_detent_center = motor.shaft_angle - current_detent_center;
#endif

            // 每一步都乘以了 snap_point 的值
            if (angle_to_detent_center > motor_config.position_width_radians * motor_config.snap_point && (motor_config.num_positions <= 0 || motor_config.position > 0))
            {
                  current_detent_center += motor_config.position_width_radians;
                  angle_to_detent_center -= motor_config.position_width_radians;
                  motor_config.position--;
                  if (dial_or_lvgl == 0)
                  {
#if XK_INVERT_ROTATION
                        dict_key(-1);
#else
                        dict_key(1);
#endif
                  }
            }
            else if (angle_to_detent_center < -motor_config.position_width_radians * motor_config.snap_point && (motor_config.num_positions <= 0 || motor_config.position < motor_config.num_positions - 1))
            {
                  current_detent_center -= motor_config.position_width_radians;
                  angle_to_detent_center += motor_config.position_width_radians;
                  motor_config.position++;
                  if (dial_or_lvgl == 0)
                  {
#if XK_INVERT_ROTATION
                        dict_key(1);
#else
                        dict_key(-1);
#endif
                  }
            }
            // 死区调整
            float dead_zone_adjustment = CLAMP(
                angle_to_detent_center,
                fminf(motor_config.position_width_radians * DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD),
                fmaxf(-motor_config.position_width_radians * DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD));

            // 出界
            bool out_of_bounds = motor_config.num_positions > 0 && ((angle_to_detent_center > 0 && motor_config.position == 0) || (angle_to_detent_center < 0 && motor_config.position == motor_config.num_positions - 1));
            motor.PID_velocity.limit = out_of_bounds ? 8 : 3;
            motor.PID_velocity.P = out_of_bounds ? motor_config.endstop_strength_unit * 4 : motor_config.detent_strength_unit * 4;

            // 处理float类型的取绝对值
            float torque = 0;
            if (fabsf(motor.shaft_velocity) > 60)
            {
                  // 如果速度太高 则不增加扭矩
                  // Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
                  Serial.println("(motor.shaft_velocity) > 60 !!!");
                  motor.move(0);
            }
            else
            {
                  // 运算符重载，输入偏差计算 PID 输出值
                  torque = motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment);
#if XK_INVERT_ROTATION
                  torque = -torque;
#endif
                  motor.move(torque);
            }
            vTaskDelay(1);
      }
}

int get_motor_position(void)
{
      return motor_config.position; // 位置
}
float get_motor_position_width_radians(void)
{
      return motor_config.position_width_radians;
}

int motor_mode[3]; // 当前模式、当前位置、上次模式
void update_motor_mode(int mode, int init_position)
{
      if (mode == MOTOR_SUPER_DIAL)
            motor_mode[2] = motor_config.position;
      motor_config = x_knob_configs[mode];
      motor_mode[1] = motor_mode[0];
      motor_mode[0] = mode;
      motor_config.position = init_position;
#if XK_INVERT_ROTATION
      current_detent_center = -motor.shaft_angle;
#else
      current_detent_center = motor.shaft_angle;
#endif
}

static bool encoder_stat = true;

bool is_encoder_enabled(void)
{
      return encoder_stat;
}

void encoder_disable(void)
{
      encoder_stat = false;
}

void encoder_enable(void)
{
      encoder_stat = true;
}

bool encoder_is_pushed(void)
{
      if (digitalRead(0) == LOW)
            return true;
      return false;
}
